package edu.centenary.centenaryController;
import java.io.*;
import java.util.*;
import java.awt.*;
import java.awt.image.*;
import javax.imageio.*;


/**
* Creates a map of the robot's perceived environment.  Walls and other obstacles are mapped as black.  Traversable terrain is green.  The location of a victim's bodypart is red.  The robot's current and previous location are blue.
*/
public class ImageWriter extends SleeperThread implements Runnable {
	 /**
	 * Width in pixels of the map image
	 */
    private int width = 1000;
	 /**
	 * Height in pixels of the map image
	 */
    private int height = 1000;
	 /**
	 * Value used to transform the world's x,y coordinates into ones used by the map
	 */
    private int scale = 20;
	 /**
	 * Sensor Data method object
	 */
    private SensorData sd;
	 /**
	 * An array list that stores the name of the limb, and it's x,y,z coordinates 
	 */
	 private ArrayList<BodyPart> parts = new ArrayList<BodyPart>();
	 
	 private ArrayList<BodyPart> people = new ArrayList<BodyPart>();
	 /**
	 * An array list that stores the waypoints, and it's x,y,z coordinates 
	 */
	 private ArrayList<WayPoint> waypoints = new ArrayList<WayPoint>();
    /** 
	 * Create a buffered image in which to draw
	 */
    private BufferedImage bufferedImage = new BufferedImage(width, height, BufferedImage.TYPE_INT_RGB);
    /**
	 * Create a graphics contents on the buffered image
	 */
    private Graphics2D g2d = bufferedImage.createGraphics();
	 
	 /*
	 * Tells the map to record the image.  Is set to false while the robot rotates since rotation seems to alter the map output.
	 */
	 private boolean recordImage = true;
	 /**
	 * The name of the sensor (odometry or INS) used to collect x,y coordinates.  Important for testing differences in reliability of the two sensors.
	 */
	 private String sensorName = "";
	 /**
	 * Creates the initial map background
	 */
	 private String name = "";
	 
    public ImageWriter(SensorData sd, ArrayList<WayPoint> waypoints, String name) {
	 	 
		  
					boolean success =  (new File("Images")).mkdir();
					
				
		  
        this.sd = sd;
		  this.waypoints = waypoints;
		  this.name = name;
        g2d.setColor(Color.yellow);
        g2d.fillRect(0,0, width, height);
		  try{
		  Scanner scan2 = new Scanner(new File("RR-people.txt"));
		  while(scan2.hasNext()){
			BodyPart bp = new BodyPart(scan2.next(),scan2.nextDouble(),scan2.nextDouble(),scan2.nextDouble());
			System.out.println(bp);
			people.add(bp);
		}
		}catch(FileNotFoundException e){
		}
		  
    }
	 /**
	 * A method to change the value of record image
	 * @param recordImage Tells the mapping methods to make the map
	 */
	 public void wipe(){
	 	g2d.setColor(Color.yellow);
      g2d.fillRect(0,0, width, height);
		

	 }
	 public void setRecord(boolean recordImage){
	 	this.recordImage = recordImage;
	 }
	 /**
	 * The method that writes the image to a file
	 */
    public void writeImage() {
        // Write generated image to a file
        try {
            // Save as JPEG
            File file = new File("Images/" + name + "INS" + System.currentTimeMillis() + ".jpg");
            ImageIO.write(bufferedImage, "jpg", file);
				

        } catch (IOException e) {
        }
    }
	 /**
	 * Writes map information for all victims found
	 */
	public void recordVictims(){
		
		for (int i = 0; i < parts.size(); i++) {
                BodyPart joe = parts.get(i);
                int x = (int)Math.round((joe.getLocationX() * scale) + width/2);
                int y = (int)Math.round((joe.getLocationY() * scale) + height/2);
                g2d.setColor(Color.red);
                g2d.fillRect(x,y,4,4);
					 
            }
			for (int i = 0; i < people.size(); i++) {
                BodyPart joe = people.get(i);
                int x = (int)Math.round((joe.getLocationX() * scale) + width/2);
                int y = (int)Math.round((joe.getLocationY() * scale) + height/2);
                g2d.setColor(Color.cyan);
                g2d.fillRect(x,y,7,7);
					 
            }
	}
	/**
	* Sets robot's waypoints
	*/
	public void setWaypoint(){
		double x = sd.getLocationX();
		double y = sd.getLocationY();
		long time = System.currentTimeMillis();
		WayPoint wp = new WayPoint(x, y, time);
		waypoints.add(wp);
		//cons.get(k).getWayPoint().add(wp);
	}
	 /**
	 * Writes map information for robot waypoints
	 */
	public void recordWaypoints(){
		
		for (int i = 0; i < waypoints.size(); i++) {
                WayPoint wei = waypoints.get(i);
                int x = (int)Math.round((wei.getX() * scale) + width/2);
                int y = (int)Math.round((wei.getY() * scale) + height/2);
                g2d.setColor(Color.blue);
                g2d.fillRect(x,y,4,4);
					 
                //System.out.println(x + " " + y);
            }
	}
	/**
	* sets the Body Part list
	*/
	public void setBodyPartList(ArrayList<BodyPart> parts){
		this.parts = parts;
	}
	/**
	* Writes map information for obstacles based on information from the range scanner sensor
	*/
	public void recordSensors(){
		//System.out.println("Your pitch is: " + sd.getPitch());
            if (sd.getPitch() <= .1 || sd.getPitch() >6.2) {
				System.out.println("Yaw" + sd.getYaw());
					//System.out.println("Printing off sensor data");
                for (int i = 0; i < sd.getRSArrayLength(); i++) {
                    double wallX = (sd.getLocationX() + (sd.getRangeScanner(i) * Math.cos((((i-90)* (Math.PI/180))- sd.getYaw())) + 0));
                    //System.out.println("Wall X: " + wallX);
                    double wallY = (sd.getLocationY() + (0 - sd.getRangeScanner(i) * Math.sin((((i-90)* (Math.PI/180)) - sd.getYaw()))));
                    //System.out.println("Wall Y: " + wallY);
                    int x = (int)Math.round((wallX * scale) + width/2);
                    //System.out.println("X: " + x);
                    int y = (int)Math.round((wallY * scale) + height/2);
                    //System.out.println("Y: " + y);
                    int drawline1 = (int)Math.round((sd.getLocationX() * scale) + width/2);
                    int drawline2 = (int)Math.round((sd.getLocationY() * scale) + height/2);
                    g2d.setColor(Color.green);
                    g2d.drawLine(drawline1, drawline2, x, y);
                    g2d.setColor(Color.black);
                    g2d.fillRect(x,y,2,2);

                }
            }
				//System.out.println("Finished Writing Sensor Data");

	}
	/**
	* Writes map information for obstacles based on information from the Odometry sensor
	*/
	
	public void recordSensors2(){
		//System.out.println("Your pitch is: " + sd.getPitch());
            if (sd.getPitch() <= .1 || sd.getPitch() >6.2) {
					//System.out.println("Printing off sensor data");
					System.out.println("OdTheta" + sd.getOdTheta());
                for (int i = 0; i < sd.getRSArrayLength(); i++) {
                    double wallX = (sd.getOdLocationX() + (sd.getRangeScanner(i) * Math.cos((((i-90)* (Math.PI/180))- sd.getOdTheta())) + 0));
                    //System.out.println("Wall X: " + wallX);
                    double wallY = (sd.getOdLocationY() + (0 - sd.getRangeScanner(i) * Math.sin((((i-90)* (Math.PI/180)) - sd.getOdTheta()))));
                    //System.out.println("Wall Y: " + wallY);
                    int x = (int)Math.round((wallX * scale) + width/2);
                    //System.out.println("X: " + x);
                    int y = (int)Math.round((wallY * scale) + height/2);
                    //System.out.println("Y: " + y);
                   

                }
            }
				//System.out.println("Finished Writing Sensor Data");

	}
	
	/**
	* Writes map information for the robot's current location
	*/
	public void recordLocation(){
		g2d.setColor(Color.blue);
	
      int x = (int)(sd.getLocationX() * scale) + width/2;
      int y = (int)(sd.getLocationY() * scale) + height/2;
      g2d.fillRect(x,y,8,8);
	
	}
	/**
	* Main method for the Image Writer method. Calls the mapping methods to create and save a map each second.
	*/
    public void run() {
		
        sleep(1000);
        recordLocation();
        //writeImage(k);

        while (!Thread.currentThread().isInterrupted()) {
		  		  if(recordImage){
				   		//Record INS Readings and create image
				   		setWaypoint();  
				   		//wipe();    
        		    		recordSensors();
							recordSensors2();
							recordVictims();
							recordWaypoints();
     		       		writeImage();
						
					}
            sleep(1000);
        }
    }
}



